/** \file Hsm.cc
 * Definitions for the class Hsm.
 *
 * (C) Copyright 2003-2006 by Semih Cemiloglu
 * All rights reserved, see COPYRIGHT file for details.
 *
 * $Id: Hsm.cc 116 2006-06-19 06:09:53Z semihc $
 *
 *
 */

#include "Hsm.hh"
#include <vector>
#include <iostream>
#include "Debug.hh"

using namespace std;
using namespace TC;


TC_DEF_FILE("$Revision: 116 $");


Hsm::~Hsm()
{
}


//! top() must not be inlined since some compilers (Sun-Forte)
//! generated incorrect ::* code.
Hsm::State_t Hsm::top(const SmEvent*)
{
  return Action_t(0);
}


Hsm::State_t Hsm::trigger(State_t st, SmSignal_t sig)
{ 
  SmEvent e(sig); 
  return st.runHsmAction(this, &e);
}


void Hsm::setup(const SmEvent* e)
{
  // HSM not executed yet
  TC_ASSERT(m_state == static_cast<Action_t>(&Hsm::top));
  TC_ASSERT(m_source != 0);

  // save m_state in a temporary
  register State_t s = m_state;        

  // top-most initial transition
  m_source.runHsmAction(this, e);

  // initial transition must go *one* level deep
  TC_ASSERT(s==trigger(m_state, HSM_EMPTY_SIG));

  // update the temporary
  s = m_state;

  // enter the state
  trigger(s, HSM_ENTRY_SIG);

  // init handled? 
  while (trigger(s, HSM_INIT_SIG) == 0) {             
    // initial transition must go *one* level deep
    TC_ASSERT(s == trigger(m_state, HSM_EMPTY_SIG));
    s = m_state;
    // enter the substate
    trigger(s, HSM_ENTRY_SIG);                  
  } // end while

}


void Hsm::dispatch(const SmEvent* e)
{
  for (m_source = m_state; 
       m_source.getAction() != 0;
       m_source = m_source.runHsmAction(this, e));
}


void Hsm::tran(State_t target)
{

  // We cannot target "top" state
  TC_ASSERT(target != static_cast<Action_t>(&Hsm::top));        

  // The state which is the super of the target
  State_t target_super;
  // The state which is the super of the source
  State_t source_super;

  register State_t s;
  register int i,j;

  // Perform exit actions while going up to the
  // transition source state
  s = m_state;
  while (s != m_source) {
    // we are about to dereference s
    TC_ASSERT(s != 0);
    State_t t = trigger(s, HSM_EXIT_SIG);
    if (t != 0) {  
      // exit action unhandled, t points to superstate
      s = t;
    }
    else {
      // exit action handled, elicit superstate
      s = trigger(s, HSM_EMPTY_SIG);
    }
  } // end while


  //TODO: Use a small object allocator here
  vector<State_t> entrypath;
  // Use a sensible default reverve
  entrypath.reserve(8);


  // Now we have to find out the path for entry to the target state.
  // Target is the last stop in the entry path.
  entrypath.push_back(target);

  // Any transtion must be one of the following cases:
  
  // Case (a)
  // check m_source == target (transition to self)
  if (m_source == target) {
    // exit source
    trigger(m_source, HSM_EXIT_SIG);                   
    goto inLCA;
  }
  
  // Case (b)
  // check m_source == target->super
  target_super = trigger(target, HSM_EMPTY_SIG);
  if (m_source == target_super) {
    goto inLCA;
  }

  // Case (c)
  // check m_source->super == target->super (most common)
  source_super = trigger(m_source, HSM_EMPTY_SIG);
  if (source_super == target_super) {
    // exit source
    trigger(m_source, HSM_EXIT_SIG);
    goto inLCA;
  }

  // Case (d)
  // check m_source->super == target
  if (source_super == target) {
    // exit source
    trigger(m_source, HSM_EXIT_SIG);
    // do not enter the LCA
    entrypath.pop_back();
    goto inLCA;
  }
   
  // Case (e)
  // check rest of m_source == target->super->super... hierarchy
  entrypath.push_back(target_super);
  for (s = trigger(target_super, HSM_EMPTY_SIG); s != 0;
       s = trigger(s, HSM_EMPTY_SIG)) {
    if (m_source == s) {
      goto inLCA;
    }
    else {
      // Record superstate of the target in the entry path
      entrypath.push_back(s);
    }
  } // end for

  
  // (e) was last of the transition topologies that do not require
  // exiting the original source state, so if the given transition
  // does not fall into this category, the source must be exited.
  trigger(m_source, HSM_EXIT_SIG);


  // Case (f)
  // check rest of m_source->super == target->super->super...
  for(i=entrypath.size()-1; i>=0; --i) {
    if(source_super == entrypath.at(i)) {
      // Remove LCA and its super states from the entry path
      for(j=entrypath.size()-1; j>=i; --j)
        entrypath.pop_back();
      goto inLCA;
    }
  }


  // Case (g)
  // check each m_source->super->super..for each target...
  for (s = source_super; s != 0; s = trigger(s, HSM_EMPTY_SIG)) {
    for (i=entrypath.size()-1; i>=0; --i) {
      if (s == entrypath.at(i)) {
        // Remove LCA and its super states from the entry path
        for(j=entrypath.size()-1; j>=i; --j)
          entrypath.pop_back();
        goto inLCA;
      }
    }
    // exit s
    trigger(s, HSM_EXIT_SIG);
  }
  
  // No known case found a transtion path, this is a malformed HSM
  TC_ASSERT(false);                                        

  // Now we are in the LCA (Least Common Ancestor) 
  // of m_source and target
 inLCA:


  // Retrace the entry path in reverse order
  for(i=entrypath.size()-1; i>=0; --i) {
    s = entrypath.at(i);
    // Enter s
    trigger(s, HSM_ENTRY_SIG);
  }

  // Update current state
  m_state = target;

  // Then complete initial transtion(s) if required
  while (trigger(target, HSM_INIT_SIG) == 0) {
    // initial transition must go *one* level deep
    TC_ASSERT(target == trigger(m_state, HSM_EMPTY_SIG));
    target = m_state;
    // enter target
    trigger(target, HSM_ENTRY_SIG);                   
  }

}
